#include "control_base/WMJRobotControl.h"

namespace wmj
{

    /**
     *@brief: 操作云台
     *
     *@param: uint8_t 0位选项, float 俯仰角, float 偏航角, float 俯仰角速度, float 偏航角速度
     */
    void WMJRobotControl::setGimbal(uint8_t mode, float pitch_angle_, float yaw_angle_, float pitch_speed_, float yaw_speed_, GIMBAL_ORIENTATION orientation = LEFT)
    {
        GCMPack control_data{};
        Buffer data_to_send{};

        float pitch_angle = pitch_angle_, 
        yaw_angle = yaw_angle_, 
        pitch_speed = pitch_speed_, 
        yaw_speed = yaw_speed_;
        // angle
        pitch_angle = pitch_angle < 0 ? pitch_angle + 2 * PI : pitch_angle;
        yaw_angle = yaw_angle < 0 ? yaw_angle + 2 * PI : yaw_angle;

        pitch_angle = pitch_angle > 2 *PI ? pitch_angle -= 2 * PI : pitch_angle;
        yaw_angle = yaw_angle > 2 *PI ? yaw_angle -= 2 * PI : yaw_angle;

        // speed
        pitch_speed = std::fabs(pitch_speed) > PI ? (pitch_speed > 0 ? PI : -PI) : pitch_speed;
        yaw_speed = std::fabs(yaw_speed) > PI ? (yaw_speed > 0 ? PI : -PI) : yaw_speed;

        int16_t s_pitch = (2047 * pitch_speed) / (2 * PI);
        int16_t s_yaw = (2047 * yaw_speed) / (2 * PI);

        s_pitch = s_pitch > 2047 ? 2047 : (s_pitch < -2048 ? -2048 : s_pitch);
        s_yaw = s_yaw > 2047 ? 2047 : (s_yaw < -2048 ? -2048 : s_yaw);

        control_data.msg.gm_mode = mode | _BaseCoor;
        control_data.msg.gm_yaw_velocity = (uint8_t)s_yaw;
        control_data.msg.gm_both_velocity = (uint8_t)((((uint16_t)s_yaw >> 8) & 0x0f) | (uint8_t)(s_pitch << 4));
        control_data.msg.gm_pitch_velocity = (uint8_t)((uint16_t)s_pitch >> 4);
        control_data.msg.gm_pitch_angle_low = (uint8_t)((uint16_t)(pitch_angle / PI * 180 / 360 * 65535));
        control_data.msg.gm_pitch_angle_high = (uint8_t)((uint16_t)(pitch_angle / PI * 180 / 360 * 65535) >> 8);
        control_data.msg.gm_yaw_angle_low = (uint8_t)((uint16_t)(yaw_angle / PI * 180 / 360 * 65535));
        control_data.msg.gm_yaw_angle_high = (uint8_t)((uint16_t)(yaw_angle / PI * 180 / 360 * 65535) >> 8);

        if(orientation == LEFT)
            data_to_send.push_back((uint8_t)Gimbal);
        else
            data_to_send.push_back((uint8_t)Right_Gimbal);
        for (auto c : control_data.data)
            data_to_send.push_back(c);

        sendFrame(data_to_send);
        // canBus->sendFrame(data_to_send);
    }

    /**
     *@brief: 获取云台姿态数据
     *
     *@return: std::vector<float> 俯仰角-偏航角-俯仰角速度-偏航角速度
     */
    std::vector<float> WMJRobotControl::getGimbal(GIMBAL_ORIENTATION orientation = LEFT)
    {
        _BaseCoor = ChassisCoor;
        uint16_t pitch_angle{}, yaw_angle{}, pitch_speed{}, yaw_speed{};
        float f_pitch_angle = 0;
        float f_yaw_angle = 0;
        float f_pitch_speed = 0;
        float f_yaw_speed = 0;
        float f_bullet_speed = 0;
        Buffer data_from_read;
        if (_BaseCoor == ChassisCoor)
        {
            if(orientation == LEFT)
                data_from_read = readFrame(Gimbal);
            else 
                data_from_read = readFrame(Right_Gimbal);

            auto bullet_speed = data_from_read[0];
            f_bullet_speed = bullet_speed / 255.f * 30;

            yaw_speed = (int16_t)((int16_t)data_from_read[1] | ((int16_t)data_from_read[2] & 0x0f) << 8);

            pitch_speed = (int16_t)(((int16_t)data_from_read[2] >> 4) | ((int16_t)data_from_read[3] << 4));
            yaw_angle = (int16_t)((int16_t)(data_from_read[4]) | ((int16_t)(data_from_read[5]) << 8));
            pitch_angle = (int16_t)((int16_t)(data_from_read[6]) | ((int16_t)(data_from_read[7]) << 8));
            f_pitch_speed = float(pitch_speed / 2048.f * 2 * PI);
            f_yaw_speed = float(yaw_speed / 2048.f * 2 * PI);
            f_pitch_speed = f_pitch_speed > 2.f * PI ? (f_pitch_speed - 4.f * PI) : f_pitch_speed;
            f_yaw_speed = f_yaw_speed > 2.f * PI ? (f_yaw_speed - 4.f * PI) : f_yaw_speed;

            f_pitch_angle = (float)pitch_angle / 65535.f * 2 * PI;
            f_yaw_angle = (float)yaw_angle / 65535.f * 2 * PI;
            f_pitch_angle = f_pitch_angle > PI ? (f_pitch_angle - 2.f * PI) : f_pitch_angle;
            f_yaw_angle = f_yaw_angle > PI ? (f_yaw_angle - 2.f * PI) : f_yaw_angle;

            float data[5] = {f_pitch_angle, f_yaw_angle, f_pitch_speed, f_yaw_speed, f_bullet_speed};
            return std::vector<float>(data, data + 5);
            // data_from_read = readFrame(Gimbal);

            // yaw_speed = (int16_t)((int16_t)data_from_read[1] | ((int16_t)data_from_read[2] & 0x0f) << 8);

            // pitch_speed = (int16_t)(((int16_t)data_from_read[2] >> 4) | ((int16_t)data_from_read[3] << 4));
            // yaw_angle = (int16_t)((int16_t)(data_from_read[4]) | ((int16_t)(data_from_read[5]) << 8));
            // pitch_angle = (int16_t)((int16_t)(data_from_read[6]) | ((int16_t)(data_from_read[7]) << 8));
            // f_pitch_speed = float(pitch_speed / 2048.f * 2 * PI);
            // f_yaw_speed = float(yaw_speed / 2048.f * 2 * PI);
            // f_pitch_speed = f_pitch_speed > 2.f * PI ? (f_pitch_speed - 4.f * PI) : f_pitch_speed;
            // f_yaw_speed = f_yaw_speed > 2.f * PI ? (f_yaw_speed - 4.f * PI) : f_yaw_speed;

            // f_pitch_angle = (float)pitch_angle / 65535.f * 2 * PI;
            // f_yaw_angle = (float)yaw_angle / 65535.f * 2 * PI;
            // f_pitch_angle = f_pitch_angle > PI ? (f_pitch_angle - 2.f * PI) : f_pitch_angle;
            // f_yaw_angle = f_yaw_angle > PI ? (f_yaw_angle - 2.f * PI) : f_yaw_angle;

            // float data[4] = {f_pitch_angle, f_yaw_angle, f_pitch_speed, f_yaw_speed};
            // return std::vector<float>(data, data + 4);
        }
        else // if(_BaseCoor == GroundCoor)
        {
            uint16_t timestamp{}, roll_angle{};
            if(orientation == LEFT)
                data_from_read = readFrame(Gyro);
            else
                data_from_read= readFrame(Right_Gyro);

            timestamp = (uint16_t)data_from_read[0] | (uint16_t)data_from_read[1] << 8;
            yaw_angle = (uint16_t)data_from_read[2] | (uint16_t)data_from_read[3] << 8;
            pitch_angle = (uint16_t)data_from_read[4] | (uint16_t)data_from_read[5] << 8;
            roll_angle = (uint16_t)data_from_read[6] | (uint16_t)data_from_read[7] << 8;

            float fpitch = (float)pitch_angle / 65535.f * 2 * PI;
            float fyaw = (float)yaw_angle / 65535.f * 2 * PI;
            float froll = (float)roll_angle / 65535.f * 2 * PI;

            wmj::GimbalPose cur_pose{fpitch, fyaw, froll};
            cur_pose.timestamp = timestamp;
            if (!poseInit)
            {
                lastpose = cur_pose;
                poseInit = true;
            }
            if (std::fabs(lastpose.yaw - cur_pose.yaw) > PI)
            {
                if (cur_pose.yaw - lastpose.yaw > 0)
                    circlecnt++;
                else
                    circlecnt--;
            }
            lastpose = cur_pose;
            cur_pose.yaw = cur_pose.yaw - circlecnt * 2.f * PI;
            cur_pose.pitch = cur_pose.pitch > PI ? cur_pose.pitch - 2 * PI : cur_pose.pitch;

            float data[4] = {cur_pose.pitch, cur_pose.yaw, 0, 0};
            return std::vector<float>(data, data + 4);
        }
    }

    /**
     * @brief 使云台以最高速度转到目标角度
     *
     * @param float 俯仰角度 float 偏航角度
     */
    void WMJRobotControl::SetGimbalAngle(float pitch_angle, float yaw_angle, GIMBAL_ORIENTATION orientation = LEFT)
    {
        setGimbal(0xB3, pitch_angle, yaw_angle, 0, 0, orientation);
    }

    /**
     * @brief 设定云台的角速度
     *
     * @param float 俯仰速度 float 偏航速度
     */
    void WMJRobotControl::SetGimbalSpeed(float pitch_speed, float yaw_speed, GIMBAL_ORIENTATION orientation = LEFT)
    {
        setGimbal(0x0f, 0, 0, pitch_speed, yaw_speed, orientation);
    }

    /**
     * @brief 设定云台的偏航角速度,俯仰角度
     *
     * @param float 俯仰角度 float 偏航速度
     */
    void WMJRobotControl::SetGimbal_YawSpeed_PitchAngle(float pitch_angle, float yaw_speed, GIMBAL_ORIENTATION orientation = LEFT)
    {
        setGimbal(0x27, pitch_angle, 0, 0, yaw_speed, orientation);
    }

    /**
     * @brief 读取云台回传的角度数据
     *
     * @return GimbalPose
     */
    GimbalPose WMJRobotControl::GetGimbalAngle(GIMBAL_ORIENTATION orientation = LEFT)
    {
        std::vector<float> gimbal_data = getGimbal(orientation);
        if (gimbal_data[3] == last_gimal_time)
        {
            usleep(400);
            gimbal_data = getGimbal(orientation);
        }
        last_gimal_time = gimbal_data[3];
        return GimbalPose{gimbal_data[0], gimbal_data[1], gimbal_data[2], gimbal_data[3]};
    }
    float WMJRobotControl::GetBulletSpeed(GIMBAL_ORIENTATION orientation)
    {
        std::vector<float> gimbal_data = getGimbal(orientation);
        // std::cout<<"bullet_speed"<< gimbal_data[4]<<std::endl;
        return gimbal_data[4];
    }
    /**
     * @brief 读取云台回传的角度数据
     *
     * @return int 0为正常数据 -1为重复数据
     */
    int WMJRobotControl::GetGimbalAngle(GimbalPose &OutputGimbalPose)
    {
        std::vector<float> gimbal_data = getGimbal();
        bool test_Flag = false; //判断是否重复读IMU信息
        bool cal_fre = false;   //判断是否计算频率
        bool m_debug = false;   //判断等待时间时候获得了新的IMU信息
        if (gimbal_data[3] == last_gimal_time)
        {
            usleep(400);
            gimbal_data = getGimbal();
            test_Flag = true;
            if (m_debug)
                std::cout << "###########等待400us##################################" << std::endl;
        }
        if ((gimbal_data[3] != last_gimal_time) && test_Flag)
        {
            if (m_debug)
                std::cout << "###########等待400us成功实现了获得IMU信息################" << std::endl;
        }
        if ((gimbal_data[3] == last_gimal_time) && (!test_Flag))
        {
            OutputGimbalPose = GimbalPose{gimbal_data[0], gimbal_data[1], gimbal_data[2], gimbal_data[3]};
            if (m_debug)
                std::cout << "###########等待400us未获得IMU信息######!!!!!!!!!!!!!!!!!" << std::endl;
            return -1;
        }
        if (cal_fre)
        {
            double IMUtime = gimbal_data[3] - last_gimal_time;
            if (IMUtime < 10000)
            {
                std::cout << "溢出来了" << std::endl; //十六位的时间戳在65534后溢出
                IMUtime = 65534 - last_gimal_time + gimbal_data[3];
            }
            double IMUFps = 1000000 / IMUtime;
            std::cout << "IMUtime : " << IMUtime << "IMUFps" << IMUFps << std::endl;
        }
        last_gimal_time = gimbal_data[3];
        // std::cout<<"gimbal_data[0]"<<gimbal_data[1]<<std::endl;
        OutputGimbalPose = GimbalPose{gimbal_data[0], gimbal_data[1], gimbal_data[2], gimbal_data[3]};
        return 0;
    }

    //##########################################暂未使用########################################
    /**
     * @brief 获取云台当前角速度值
     *
     * @return
     */
    GimbalPose WMJRobotControl::GetGimbalSpeed(GIMBAL_ORIENTATION orientation = LEFT)
    {
        std::vector<float> gimbal_data = getGimbal(orientation);
        return GimbalPose{gimbal_data[2], gimbal_data[3]};
    }

/**************************新加的**************************************/
    GimbalPose WMJRobotControl::getGimbalPoseAngle(GIMBAL_ORIENTATION orientation = LEFT)
    {
        Buffer data_from_read;
        if(orientation == LEFT)
            data_from_read = readFrame(BodyLeftGimbalAngle);
        else
            data_from_read = readFrame(BodyRightGimbalAngle);
        uint16_t yaw_angle_16u = (uint16_t)data_from_read[2] | ((uint16_t)data_from_read[3]) << 8;
        uint16_t pitch_angle_16u = (uint16_t)data_from_read[4] | ((uint16_t)data_from_read[5]) << 8;
        float pitch_angle = (float)pitch_angle_16u / 65535.0f * 2 * PI;
        float yaw_angle = (float)yaw_angle_16u / 65535.0f * 2 * PI;

        pitch_angle = pitch_angle > PI ? pitch_angle - 2 * PI : pitch_angle;
        
        return GimbalPose(pitch_angle, yaw_angle);
    }

    GimbalPose WMJRobotControl::getGimbalPoseSpeed(GIMBAL_ORIENTATION orientation)
    {
        Buffer data_from_read;
        if(orientation == LEFT)
            data_from_read = readFrame(BodyLeftGimbalSpeed);
        else
            data_from_read = readFrame(BodyRightGimbalSpeed);
        uint16_t yaw_speed_16u = (uint16_t)data_from_read[2] | ((uint16_t)data_from_read[3]) << 8;
        uint16_t pitch_speed_16u = (uint16_t)data_from_read[4] | ((uint16_t)data_from_read[5]) << 8;
        
        float pitch_speed = (double)pitch_speed_16u / 32768.0 * 2 * PI;
        float yaw_speed = (double)yaw_speed_16u / 32768.0 * 2 * PI;

        pitch_speed = pitch_speed > PI ? pitch_speed - 2 * PI : pitch_speed;
        yaw_speed = yaw_speed > PI ? yaw_speed - 2 * PI : yaw_speed;

        return GimbalPose(pitch_speed, yaw_speed);
    }

    std::vector<float> WMJRobotControl::getGimbalRotationRange(GIMBAL_ORIENTATION orientation)
    {
        Buffer data_from_read;
        if(orientation == LEFT)
            data_from_read = readFrame(Gyro);
        else
            data_from_read = readFrame(Right_Gyro);
        float max_range = (double)data_from_read[6] / 255.0 * 2 * PI;
        float min_range = (double)data_from_read[7] / 255.0 * 2 * PI;

        return {max_range, min_range};
    }

    std::vector<float> WMJRobotControl::getBigYaw()
    {
        Buffer data_from_read;
        data_from_read = readFrame(BigYaw);

        uint8_t body_angle_16u = (uint16_t)data_from_read[0] | ((uint16_t)data_from_read[1]) << 8;
        uint8_t body_speed_16u = (uint16_t)data_from_read[2] | ((uint16_t)data_from_read[3]) << 8;
        uint8_t gyro_angle_16u = (uint16_t)data_from_read[4] | ((uint16_t)data_from_read[5]) << 8;
        uint8_t gyro_speed_16u = (uint16_t)data_from_read[6] | ((uint16_t)data_from_read[7]) << 8;

        float body_angle = (double)body_angle_16u / 65535.0 * 2 * PI;
        float body_speed = (double)body_speed_16u / 65535.0 * 2 * PI;
        float gyro_angle = (double)gyro_angle_16u / 65535.0 * 2 * PI;
        float gyro_speed = (double)gyro_speed_16u / 65535.0 * 2 * PI;

        body_angle = body_angle > PI ? body_angle - 2 * PI : body_angle;
        body_speed = body_speed > PI ? body_speed - 2 * PI : body_speed;
        gyro_speed = gyro_speed > PI ? gyro_speed - 2 * PI : gyro_speed;

        return {body_angle, gyro_angle, body_speed, gyro_speed};
    }

    std::vector<float> WMJRobotControl::getBigYaw2()
    {
        Buffer data_from_read;
        data_from_read = readFrame(Gyro);

        uint16_t yaw_angle = (uint16_t)data_from_read[4] | (uint16_t)data_from_read[5] << 8;

        float fyaw = (float)yaw_angle / 65535.f * 2 * PI;

        float data[4] = {0, fyaw, 0, 0};
        return std::vector<float>(data, data + 4);
    }

    void WMJRobotControl::setBigYaw(uint8_t big_yaw_flag, float value)
    {
        Buffer data_to_send;
        uint16_t speed_16u, angle_16u;
        float angle, speed;
        if(big_yaw_flag & 0x08) // speed enable
        {
            angle_16u = 0;
            speed = value > PI ? (value > PI ? PI : -PI) : value; //归到-PI到PI
            int32_t speed_32i = (32768 * speed) / (2 * PI);
            speed_32i = speed_32i >= 32767 ? 32767 : speed_32i;
            speed_32i = speed_32i <= -32768 ? 32768 : speed_32i;
            speed_16u = speed_32i;
        }
        else if(big_yaw_flag & 0x04) // angle enable
        {
            angle = value;
            angle = angle < 0 ? angle += 2 * PI : angle;
            angle = angle > 2 * PI ? angle -= 2 * PI : angle;
            angle_16u = (angle / (2 * PI) * 65535);
            speed_16u = 0;
        }
        else
        {
            angle_16u = 0;
            speed_16u = 0;
        }

        data_to_send.push_back(BigYaw);
        data_to_send.push_back(0x00);
        data_to_send.push_back(angle_16u & 0xff);
        data_to_send.push_back(angle_16u >> 8);
        data_to_send.push_back(speed_16u & 0xff);
        data_to_send.push_back(speed_16u >> 8);
        data_to_send.resize(9, 0);

        sendFrame(data_to_send);
    }

    void WMJRobotControl::setSentryCmd(bool comfirm_respawn, bool apply_instant_respawn,
    int apply_bullet_redeem_num, int apply_remote_supply_times, int apply_remote_heal_times)
    {
        Buffer data_to_send;
        data_to_send.push_back(BigYaw);
        data_to_send.push_back(0x01);
        if(comfirm_respawn) data_to_send.push_back(0x01);
        else data_to_send.push_back(0x00);
        if(apply_instant_respawn) data_to_send.push_back(0x01);
        else data_to_send.push_back(0x00);
        data_to_send.push_back(apply_bullet_redeem_num & 0xff);
        data_to_send.push_back(apply_bullet_redeem_num >> 8);
        data_to_send.push_back(apply_remote_supply_times);
        data_to_send.push_back(apply_remote_heal_times);
        
        data_to_send.resize(9, 0);
        sendFrame(data_to_send);
    }


}
